//% color="#FF6B35" iconWidth=50 iconHeight=40
namespace BoosterRobotics {
    
    //% block="Initialize robot" blockType="command"
    export function initRobot(parameter: any, block: any) {
        Generator.addImport("from booster_robotics import *");
        Generator.addImport("import time");
        Generator.addInit("initRobot",`
ChannelFactory.Instance().Init(0, "lo")
robot_client = B1LocoClient()
robot_client.Init()
print("Robot initialized successfully")`);
    }

    //% block="模式切换" blockType="tag"
    export function separator1() {}
    //% block="Change robot mode to [MODE]" blockType="command"
    //% MODE.shadow="dropdown" MODE.options="ROBOT_MODES"
    export function changeMode(parameter: any, block: any) {
        let mode = parameter.MODE.code;
        Generator.addCode(`time.sleep(0.5)`);
        Generator.addCode(`result = robot_client.ChangeMode(${mode})`);
        Generator.addCode(`time.sleep(0.5)
gm: GetModeResponse = GetModeResponse()
res = robot_client.GetMode(gm)`);
    }


    //% block="Get mode" blockType="reporter"
    export function getMode(parameter: any, block: any) {
        Generator.addInit("initRobot1",`time.sleep(0.5)
gm: GetModeResponse = GetModeResponse()
res = robot_client.GetMode(gm)`);
        Generator.addCode(`gm.mode`);
    }





    //% block="运行操作" blockType="tag"
    export function separator2() {}

    //% block="Move robot X:[X] Y:[Y] Z:[Z]" blockType="command"
    //% X.shadow="number" X.defl=0.0
    //% Y.shadow="number" Y.defl=0.0
    //% Z.shadow="number" Z.defl=0.0
    export function moveRobot(parameter: any, block: any) {
        let x = parameter.X.code;
        let y = parameter.Y.code;
        let z = parameter.Z.code;
        Generator.addCode(`time.sleep(0.5)`);
        Generator.addCode(`result = robot_client.Move(${x}, ${y}, ${z})`);
    }

    //% block="Move robot [DIRECTION] [SPEED] for [TIME] seconds" blockType="command"
    //% DIRECTION.shadow="dropdown" DIRECTION.options="MOVE_DIRECTIONS"
    //% SPEED.shadow="range" SPEED.params.min=0.1 SPEED.params.max=2.0 SPEED.defl=0.8
    //% TIME.shadow="number" TIME.defl=1.0
    export function moveDirection(parameter: any, block: any) {
        let direction = parameter.DIRECTION.code;
        let speed = parameter.SPEED.code;
        let moveTime = parameter.TIME.code;
        Generator.addCode(`time.sleep(0.5)`);
        if (direction==="1"){
            Generator.addCode(`x, y, z = ${speed}, 0.0, 0.0`);
        }else if (direction==="2"){
            Generator.addCode(`x, y, z = -${speed}, 0.0, 0.0`);
        }else if (direction==="3"){
            Generator.addCode(`x, y, z = 0.0, ${speed}, 0.0`);
        }else if (direction==="4"){
            Generator.addCode(`x, y, z = 0.0, -${speed},0.0`)
        }else if (direction==="5"){
            Generator.addCode(`x, y, z = 0.0, 0.0, ${speed}`);
        }else if (direction==="6"){
            Generator.addCode(`x, y, z = 0.0, 0.0, -${speed}`);
        }
        Generator.addCode(`result = robot_client.Move(x, y, z)`);
        Generator.addCode(`time.sleep(${moveTime})
result = robot_client.Move(0.0, 0.0, 0.0)`);
    }
    //% block="Stop robot" blockType="command"
    export function stopRobot(parameter: any, block: any) {
        Generator.addCode(`result = robot_client.Move(0.0, 0.0, 0.0)`);
    }

    //% block="Wave hand [ACTION]" blockType="command"
    //% ACTION.shadow="dropdown" ACTION.options="WAVE_HAND_ACTIONS"
    export function waveHand(parameter: any, block: any) {
        let action = parameter.ACTION.code;
        Generator.addCode(`result = robot_client.WaveHand(${action})`);
    }

    //% block="Handshake [ACTION]" blockType="command"
    //% ACTION.shadow="dropdown" ACTION.options="HANDSHAKE_ACTIONS"
    export function handshake(parameter: any, block: any) {
        let action = parameter.ACTION.code;
        Generator.addCode(`result = robot_client.Handshake(${action})`);
    }

    //% block="静态操作" blockType="tag"
    export function separator4() {}

    //% block="Rotate robot head pitch:[PITCH] yaw:[YAW]" blockType="command"
    //% PITCH.shadow="range" PITCH.params.min=-170 PITCH.params.max=57 PITCH.defl=0.0
    //% YAW.shadow="range" YAW.params.min=-45 YAW.params.max=45 YAW.defl=0.0
    export function rotateHead(parameter: any, block: any) {
        let pitch = parameter.PITCH.code;
        let yaw = parameter.YAW.code;
        Generator.addImport("import math");
      
        Generator.addCode(`result = robot_client.RotateHead(round(${pitch} * math.pi / 180, 3), round(${yaw} * math.pi / 180, 3))`);
    }

    // //% block="Create hand posture X:[X] Y:[Y] Z:[Z]" blockType="command"
    // //% X.shadow="number" X.defl=0.35
    // //% Y.shadow="number" Y.defl=0.55
    // //% Z.shadow="number" Z.defl=0.1
    // export function createHandPosture(parameter: any, block: any) {
    //     let x = parameter.X.code;
    //     let y = parameter.Y.code;
    //     let z = parameter.Z.code;
    //     Generator.addCode(`tar_posture = Posture()`);
    //     Generator.addCode(`tar_posture.position = Position(${x}, ${y}, ${z})`);
    // }

    // //% block="Set hand orientation roll:[ROLL] pitch:[PITCH] yaw:[YAW]" blockType="command"
    // //% ROLL.shadow="number" ROLL.defl=-1.57
    // //% PITCH.shadow="number" PITCH.defl=-1.57
    // //% YAW.shadow="number" YAW.defl=0.0
    // export function setHandOrientation(parameter: any, block: any) {
    //     let roll = parameter.ROLL.code;
    //     let pitch = parameter.PITCH.code;
    //     let yaw = parameter.YAW.code;
    //     Generator.addCode(`tar_posture.orientation = Orientation(${roll}, ${pitch}, ${yaw})`);
    // }

    // //% block="Move hand end effector duration:[DURATION] hand:[HAND]" blockType="command"
    // //% DURATION.shadow="number" DURATION.defl=2000
    // //% HAND.shadow="dropdown" HAND.options="HAND_SELECTION"
    // export function moveHandEndEffector(parameter: any, block: any) {
    //     let duration = parameter.DURATION.code;
    //     let hand = parameter.HAND.code;
    //     Generator.addCode(`time.sleep(0.5)`);
    //     Generator.addCode(`result = robot_client.MoveHandEndEffectorV2(tar_posture, ${duration}, ${hand})`);
    // }

    //% block="结果操作" blockType="tag"
    export function separator5() {}

    //% block="Get last operation result" blockType="reporter"
    export function getResult(parameter: any, block: any) {
        Generator.addCode([`result`, Generator.ORDER_ATOMIC]);
    }



}